#include "FbxBone.h"

namespace GC
{
	FbxBone::FbxBone(const FbxCluster* a_cluster, FbxNode* a_boneNode ,const fbxsdk_2014_1::FbxMesh* a_mesh, const FbxNode* a_rootNode)
		: m_id( FbxHelper::GetNodeID( a_boneNode ) ),
		m_name( a_boneNode->GetName() ),
		m_linkIndicesCount( a_cluster->GetControlPointIndicesCount() )
	{
		const FbxNode* f_parentNode = a_boneNode->GetParent();
		//Check if parent exists and isn't rootnode
		if (f_parentNode != NULL && f_parentNode != a_rootNode)
		{
			m_parentName = FbxString( f_parentNode->GetName() );
			m_parentID = FbxHelper::GetNodeID( f_parentNode );
		}
		else
		{
			//If parent didn't exist set it to -1
			m_parentID = -1;
		}

		m_bindTransform = FbxHelper::GetDQorMatrix4x4();
		m_transform = FbxHelper::GetDQorMatrix4x4();

		int* f_linkIndices = a_cluster->GetControlPointIndices();
		double* f_weightValues = a_cluster->GetControlPointWeights();

		//Need to copy each value from the 2 arrays above because the reference is destroyed when m_manager->Destroy() happens
		m_linkIndices.reserve(m_linkIndicesCount);
		m_weightValues.reserve(m_linkIndicesCount);

		for (int i = 0; i < m_linkIndicesCount; ++i)
		{	
			m_linkIndices.push_back(*(f_linkIndices + i));
			m_weightValues.push_back(*(f_weightValues + i));
			
		}
		
		SetGlobalPosition(a_cluster, a_boneNode, a_mesh, a_rootNode);
	}
	/* Get functions for example the exporter */
	/// <summary>
	/// Get the unique node ID for this bone
	/// </summary>
	/// <returns></returns>
	const int FbxBone::GetID() const
	{
		return m_id;
	}
	const FbxString FbxBone::GetName() const
	{		
		return m_name;
	}
	/// <summary>
	/// Returns -1 if there is no parent
	/// </summary>
	/// <returns></returns>
	const int FbxBone::GetParentID() const
	{
		return m_parentID;
	}
	const FbxString FbxBone::GetParentName() const
	{
		return m_parentName;
	}

	const VectorConstDouble FbxBone::GetTransform() const
	{
		return m_transform;
	}
	const VectorConstDouble FbxBone::GetBindTransform() const
	{
		return m_bindTransform;
	}

	const size_t FbxBone::GetLinkIndicesSize() const
	{
		return m_linkIndices.size();
	}
	const VectorConstInt FbxBone::GetLinkIndices() const
	{
		return m_linkIndices;
	}
	const VectorConstDouble FbxBone::GetWeightValues() const
	{
		return m_weightValues;
	}

	/* PRIVATE FUNCTIONS */

	/// <summary>
	/// Calculates the localTransform and bindTransform
	/// </summary>
	/// <param name="a_cluster">The cluster.</param>
	/// <param name="a_boneNode">The fbx node.</param>
	/// <param name="a_mesh">The fbx mesh.</param>
	/// <param name="a_rootNode">The root node</param>
	void FbxBone::SetGlobalPosition(const FbxCluster* a_cluster, FbxNode* a_boneNode, const fbxsdk_2014_1::FbxMesh* a_mesh, const FbxNode* a_rootNode)
	{
		/* Initialize all the different matrixes needed */
		FbxAMatrix f_transformMatrix;

		FbxAMatrix f_referenceGeometry;
		FbxAMatrix f_referenceGlobalInitPosition;
		FbxAMatrix f_referenceGlobalCurrentPosition;

		FbxAMatrix f_clusterGeometry;
		FbxAMatrix f_clusterGlobalInitPosition;
		FbxAMatrix f_clusterGlobalCurrentPosition;
		FbxAMatrix f_clusterRelativeInitPosition;
		FbxAMatrix f_clusterRelativeCurrentPositionInverse;		

		//Global position Matrix
		FbxAMatrix f_globalPosition;
		FbxAMatrix f_globalParentPosition;
		FbxAMatrix f_globalOffPosition;

		FbxAMatrix f_geometryOffset;

		const FbxHelper::FbxPoseData f_poseData( a_boneNode );

		if (f_poseData.m_pose != nullptr)
		{
			const FbxNode* f_meshNode = nullptr;
				
			if (a_mesh->GetNodeCount() > 0)
			{
				f_meshNode = a_mesh->GetNode();

				f_globalPosition = FbxHelper::GetGlobalPosition(a_boneNode, f_poseData.m_pose, &f_globalParentPosition);
				f_geometryOffset = FbxHelper::GetGeometry( f_meshNode );					
				f_globalOffPosition = f_globalPosition * f_geometryOffset;
				f_referenceGeometry = FbxHelper::GetGeometry( f_meshNode );
			}
			//Reference matrixes
			a_cluster->GetTransformMatrix( f_referenceGlobalInitPosition );
			//Seemed to get an idendity matrix when I had f_globalPosition
			f_referenceGlobalCurrentPosition = f_globalOffPosition; 
			// Multiply f_referenceGlobalInitPosition by Geometric Transformation
			f_referenceGlobalInitPosition *= f_referenceGeometry;

			//Cluster matrixes
			a_cluster->GetTransformLinkMatrix( f_clusterGlobalInitPosition );
			//a_pose parameter is NULL because result will be identity matrix if it isn't
			f_clusterGlobalCurrentPosition = FbxHelper::GetGlobalPosition( a_boneNode, nullptr, nullptr );
			// Compute the initial position of the link relative to the reference.
			f_clusterRelativeInitPosition = f_clusterGlobalInitPosition.Inverse() * f_referenceGlobalInitPosition;
			// Compute the current position of the link relative to the reference.
			f_clusterRelativeCurrentPositionInverse = f_referenceGlobalCurrentPosition.Inverse() * f_clusterGlobalCurrentPosition;
			// Compute the shift of the link relative to the reference.
			f_transformMatrix = f_referenceGlobalCurrentPosition.Inverse();

			//Set the now calculated values!
			FbxHelper::SetDQorMat4x4FromMat( f_transformMatrix, m_transform );
			FbxHelper::SetDQorMat4x4FromMat( f_clusterRelativeInitPosition, m_bindTransform);
		}			
	}
}